#include "ti_msp_dl_config.h"
#include "GAY_GET.h"
#include "Motor.h"
#include "delay.h"
int get_gay() {   
    int tack = 0;  
    // 初始化所有TRACKx为0（避免受上次值影响）  
    TRACK1 = TRACK2 = TRACK3 = TRACK4 = TRACK5 = TRACK6 = TRACK7 = 0;  

    // 检测引脚，高电平则TRACKx保持0，低电平则设为1  
    if(DL_GPIO_readPins(GAY_GET1_PORT, GAY_GET1_GAY_1_PIN) == 0) {  
        TRACK6 = 1;  
    }  
    if(DL_GPIO_readPins(GAY_GET2_PORT, GAY_GET2_GAY_2_PIN) == 0) {  
        TRACK7 = 1;  
    }  
    if(DL_GPIO_readPins(GAY_GET3_PORT, GAY_GET3_GAY_3_PIN) == 0) {  
        TRACK1 = 1;  
    }  
    if(DL_GPIO_readPins(GAY_GET4_PORT, GAY_GET4_GAY_4_PIN) == 0) {  
        TRACK2 = 1;  
    }  
    if(DL_GPIO_readPins(GAY_GET5_PORT, GAY_GET5_GAY_5_PIN) == 0) {  
        TRACK3 = 1;  
    }  
    if(DL_GPIO_readPins(GAY_GET6_PORT, GAY_GET6_GAY_6_PIN) == 0) {  
        TRACK4 = 1;  
    }  
    if(DL_GPIO_readPins(GAY_GET7_PORT, GAY_GET7_GAY_7SSS_PIN) == 0) {  
        TRACK5 = 1;  
    }  

    tack = TRACK1*1 + TRACK2*2 + TRACK3*4 + TRACK4*8 + TRACK5*16 + TRACK6*32 + TRACK7*64;  
    return tack;  
}

/*
void pid_gary(uint8_t pid_tack)
{
	float err,OUT,kp=5;
	switch(tack)
		{case 0x77: err = 0;break;  
		 case 0x67: err = 2.5;break;  //左1，左2检测到灰度
         case 0x6f: err = 3.5;break;  
         case 0x5f: err = 5;break;  
         case 0x3f: err = 6;break;  
         case 0x4f: err = 4.5;break;  
         case 0x47: err = 10;break;  
         case 0x07: err = 10;break;  //中 左1 左2 左3检测到灰度 
         case 0x0f: err = 10;break; 
         case 0x1f: err = 10;break;  //err  = 10 转直角 
         case 0x7b: err = -3.5;break;  
         case 0x73: err = -2.5;break; 
         case 0x71: err = -5;break;   
         case 0x7d: err = -6;break; 
         case 0x7e: err = -8;break;                                                        
	    }
	OUT = kp* err;
	set_1motorspeed(1600+OUT);
	set_2motorspeed(2000-OUT);		
}
*/

/**
 * @brief 灰度传感器PID控制函数（优化版）
 * @param pid_tack 灰度传感器状态编码（0x00-0x7F）
 * @note 基于传感器状态计算偏差，通过比例控制调节电机速度，实现循迹
 */
 
extern   int time;
extern   uint8_t quan;

 extern  float kp ;  // 比例系数（可根据实际调试调整）
 extern  float ki ;  // 比例系数（可根据实际调试调整）
 extern  float kd ;  // 比例系数（可根据实际调试调整）
 extern  int turn;
void pid_gary(uint8_t pid_tack)
{
    int i = 0;
    int flag = 1;
    float err = 0.0f;       // 轨迹偏差（正数：右偏，负数：左偏）
    float out = 0.0f;       // PID输出（速度调节量）
    // 统计检测到黑线的传感器数量（TRACKx=0表示黑线）
    static float last_err;
    int black_count = 0;

    // 1. 先根据传感器状态计算黑线数量（核心：识别任意3路黑线）
    // 解析pid_tack，判断哪些TRACKx为0（黑线）
    if ((pid_tack & 0x01) == 0) black_count++;  // TRACK1=0（黑线）
    if ((pid_tack & 0x02) == 0) black_count++;  // TRACK2=0
    if ((pid_tack & 0x04) == 0) black_count++;  // TRACK3=0
    if ((pid_tack & 0x08) == 0) black_count++;  // TRACK4=0
    if ((pid_tack & 0x10) == 0) black_count++;  // TRACK5=0
    if ((pid_tack & 0x20) == 0) black_count++;  // TRACK6=0
    if ((pid_tack & 0x40) == 0) black_count++;  // TRACK7=0

    // 2. 若检测到任意3路黑线，强制err=10.0f（触发直角转向）
    if (black_count >= 3) {
        err = 10.0f;
    } 
    else {
    switch(pid_tack)
    {
        case 0x77:  // 中间传感器检测到，轨迹居中
            err = 0.0f;
            break;  
        case 0x67:  // 左1、左2检测到，轻微右偏
            err = 2.5f;
            break;  
        case 0x6f:  // 左1、左2、左3检测到，中度右偏
            err = 3.5f;
            break;  
        case 0x5f:  // 左2、左3、左4检测到，较严重右偏
            err = 5.0f;
            break;  
        case 0x3f:  // 转直角
            err = 6.0f;
            break;  
        case 0x4f:  // 左2、左3、左4、左5检测到，中度右偏
            err = 4.5f;
            break;  
        case 0x47: err = -5.0f;break; // 左1、左2、左3、左4检测到，需大幅右偏纠正
        case 0x07:  err = 10.0f;break;  // 中、左1、左2、左3检测到，需大幅右偏纠正 ////
        case 0x0f:  err = 10.0f; break;// 左2、左3、左4、左5检测到，需大幅右偏纠正 /////
        case 0x01:  err = 10.0f; break;// 左2、左3、左4、左5检测到，需大幅右偏纠正 /////        
        case 0x1f: err = 6.0f;break; // 左3、左4、左5、左6检测到，需直角
        case 0x7b:  // 右1、右2检测到，轻微左偏
            err = -3.5f;
            break;  
        case 0x73:  // 右2、右3检测到，轻微左偏
            err = -2.5f;
            break; 
        case 0x71:  // 右3、右4检测到，较严重左偏
            err = -5.0f;
            break;   
        case 0x7d:  // 右4、右5检测到，严重左偏
            err = -6.0f;
            break; 
        case 0x7e:  // 右5、右6检测到，需大幅左偏纠正
            err = -8.0f;
            break;  
        default:    // 未定义的传感器状态（如全白、全黑），默认无偏差
            err = 0.0f;
            break;                                                        
    }
    }

    // 计算比例控制输出
    out = kp * err;
    if(err == 10.0f && i == 0 )//转直角度
    {  
        turn = 0;
        quan++;
        flag = 2;
        i = 1;
    }
    else if (err != 10.0f)
     {
        i = 0;
    }

    if(flag == 1)
    {
    // 输出限幅（保护电机，避免速度突变过大，范围可根据实际调整）
    if (out > 800.0f)  out = 800.0f;   // 最大右偏调节量
    if (out < -800.0f) out = -800.0f;  // 最大左偏调节量

    // 计算左右电机目标速度并设置（左电机加调节量，右电机减调节量，实现转向）
    int left_speed  = 2100 + (int)out;
    int right_speed = 2500 - (int)out;

    // 设置电机速度（底层函数会处理速度范围限制）
    set_1motorspeed(left_speed);
    set_2motorspeed(right_speed);
    }
}
/*
//七路灰度
int get_gay()
{   int tack = 0;
    if(DL_GPIO_readPins(GAY_GET1_PORT,GAY_GET1_GAY_1_PIN) == 0)
    {
          TRACK6 = 1;  
    }
    if (DL_GPIO_readPins(GAY_GET2_PORT,GAY_GET2_GAY_2_PIN) ==0) {
          TRACK7 = 1;
    
    }
    if(DL_GPIO_readPins(GAY_GET3_PORT,GAY_GET3_GAY_3_PIN) == 0)
    {
          TRACK1 = 1;  
    }
    if (DL_GPIO_readPins(GAY_GET4_PORT,GAY_GET4_GAY_4_PIN) ==0) {
          TRACK2 = 1;
    
    }
    if(DL_GPIO_readPins(GAY_GET5_PORT,GAY_GET5_GAY_5_PIN) == 0)
    {
          TRACK3 = 1;  
    }
    if (DL_GPIO_readPins(GAY_GET6_PORT,GAY_GET6_GAY_6_PIN) ==0) {
          TRACK4 = 1;
    } 
    if (DL_GPIO_readPins(GAY_GET7_PORT,GAY_GET7_GAY_7SSS_PIN) ==0) {
           TRACK5 = 1;
    
    }
      tack = TRACK1 *1 +  TRACK2 * 2 + TRACK3* 4+TRACK4 * 8+ TRACK5*16+TRACK6*32 + TRACK7*64; 
        return tack;
}
*/